package jp.seraph.jsade.model.nao;

import java.util.HashMap;
import java.util.Iterator;
import java.util.Map;

import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.message.MessageUtil;
import jp.seraph.jsade.model.Align;
import jp.seraph.jsade.model.JointIdentifier;

/**
 * Nao用のJointIdentifier<br>
 *
 */
public class NaoJointIdentifier implements JointIdentifier {
    public NaoJointIdentifier(String aName, Angle aMinAngle, Angle aMaxAngle, Align aAlign, boolean aShouldMinus) {
        mName = aName;
        mMaxAngle = aMaxAngle;
        mMinAngle = aMinAngle;
        mAlign = aAlign;
        mShouldMinus = aShouldMinus;
        if (mName.startsWith("l"))
            mMirrorIdentifier = "r" + mName.substring(1);
        else if (mName.startsWith("r"))
            mMirrorIdentifier = "l" + mName.substring(1);
        else
            mMirrorIdentifier = mName;

        IDENTIFIER_MAP.put(mName, this);
    }

    private String mName;

    private Angle mMaxAngle;
    private Angle mMinAngle;

    private Align mAlign;

    private boolean mShouldMinus;

    private String mMirrorIdentifier;

    private static Map<String, NaoJointIdentifier> IDENTIFIER_MAP = new HashMap<String, NaoJointIdentifier>();

    public static NaoJointIdentifier getJointIdentifier(String aName) {
        NaoJointIdentifier tResult = IDENTIFIER_MAP.get(aName);
        if (tResult == null)
            throw new IllegalArgumentException(MessageUtil.noJointFound(aName));

        return tResult;
    }

    public static Iterable<NaoJointIdentifier> getIdentifiers() {
        return IDENTIFIER_MAP.values();
    }

    public static Iterator<NaoJointIdentifier> getIterator() {
        return IDENTIFIER_MAP.values().iterator();
    }

    /**
     * 首左右回転関節
     */
    public static NaoJointIdentifier HJ1 = new NaoJointIdentifier("hj1", Angle.createAngleAsDegree(-120), Angle.createAngleAsDegree(120), Align.CENTER, false);

    /**
     * 首上下回転関節
     */
    public static NaoJointIdentifier HJ2 = new NaoJointIdentifier("hj2", Angle.createAngleAsDegree(-45), Angle.createAngleAsDegree(45), Align.CENTER, false);

    /**
     * 右肩上下回転関節
     */
    public static NaoJointIdentifier RAJ1 = new NaoJointIdentifier("raj1", Angle.createAngleAsDegree(-120), Angle.createAngleAsDegree(120), Align.RIGHT, false);

    /**
     * 右肩左右回転関節
     */
    public static NaoJointIdentifier RAJ2 = new NaoJointIdentifier("raj2", Angle.createAngleAsDegree(-95), Angle.createAngleAsDegree(1), Align.RIGHT, true);

    /**
     * 右肘のねじり関節
     */
    public static NaoJointIdentifier RAJ3 = new NaoJointIdentifier("raj3", Angle.createAngleAsDegree(-120), Angle.createAngleAsDegree(120), Align.RIGHT, true);

    /**
     * 右肘の折りたたみ関節
     */
    public static NaoJointIdentifier RAJ4 = new NaoJointIdentifier("raj4", Angle.createAngleAsDegree(-1), Angle.createAngleAsDegree(90), Align.RIGHT, true);

    /**
     * 左肩上下回転関節
     */
    public static NaoJointIdentifier LAJ1 = new NaoJointIdentifier("laj1", Angle.createAngleAsDegree(-120), Angle.createAngleAsDegree(120), Align.LEFT, false);

    /**
     * 左肩左右回転関節
     */
    public static NaoJointIdentifier LAJ2 = new NaoJointIdentifier("laj2", Angle.createAngleAsDegree(-1), Angle.createAngleAsDegree(95), Align.LEFT, true);

    /**
     * 左肘のねじり関節
     */
    public static NaoJointIdentifier LAJ3 = new NaoJointIdentifier("laj3", Angle.createAngleAsDegree(-120), Angle.createAngleAsDegree(120), Align.LEFT, true);

    /**
     * 左肘の折りたたみ関節
     */
    public static NaoJointIdentifier LAJ4 = new NaoJointIdentifier("laj4", Angle.createAngleAsDegree(-95), Angle.createAngleAsDegree(1), Align.LEFT, true);

    /**
     * 右股ねじり関節
     */
    public static NaoJointIdentifier RLJ1 = new NaoJointIdentifier("rlj1", Angle.createAngleAsDegree(-90), Angle.createAngleAsDegree(1), Align.RIGHT, false);

    /**
     * 右股開脚関節
     */
    public static NaoJointIdentifier RLJ2 = new NaoJointIdentifier("rlj2", Angle.createAngleAsDegree(-45), Angle.createAngleAsDegree(25), Align.RIGHT, true);

    /**
     * 右太腿上下回転関節
     */
    public static NaoJointIdentifier RLJ3 = new NaoJointIdentifier("rlj3", Angle.createAngleAsDegree(-25), Angle.createAngleAsDegree(100), Align.RIGHT, false);

    /**
     * 右ひざ折りたたみ関節
     */
    public static NaoJointIdentifier RLJ4 = new NaoJointIdentifier("rlj4", Angle.createAngleAsDegree(-130), Angle.createAngleAsDegree(1), Align.RIGHT, false);

    /**
     * 右足首上下（前後）回転関節
     */
    public static NaoJointIdentifier RLJ5 = new NaoJointIdentifier("rlj5", Angle.createAngleAsDegree(-45), Angle.createAngleAsDegree(75), Align.RIGHT, false);

    /**
     * 右足首左右回転関節
     */
    public static NaoJointIdentifier RLJ6 = new NaoJointIdentifier("rlj6", Angle.createAngleAsDegree(-25), Angle.createAngleAsDegree(45), Align.RIGHT, true);

    /**
     * 左股ねじり関節
     */
    public static NaoJointIdentifier LLJ1 = new NaoJointIdentifier("llj1", Angle.createAngleAsDegree(-90), Angle.createAngleAsDegree(1), Align.LEFT, false);

    /**
     * 左股開脚関節
     */
    public static NaoJointIdentifier LLJ2 = new NaoJointIdentifier("llj2", Angle.createAngleAsDegree(-25), Angle.createAngleAsDegree(45), Align.LEFT, true);

    /**
     * 左太腿上下回転関節
     */
    public static NaoJointIdentifier LLJ3 = new NaoJointIdentifier("llj3", Angle.createAngleAsDegree(-25), Angle.createAngleAsDegree(100), Align.LEFT, false);

    /**
     * 左ひざ折りたたみ関節
     */
    public static NaoJointIdentifier LLJ4 = new NaoJointIdentifier("llj4", Angle.createAngleAsDegree(-130), Angle.createAngleAsDegree(1), Align.LEFT, false);

    /**
     * 左足首上下（前後）回転関節
     */
    public static NaoJointIdentifier LLJ5 = new NaoJointIdentifier("llj5", Angle.createAngleAsDegree(-45), Angle.createAngleAsDegree(75), Align.LEFT, false);

    /**
     * 左足首左右回転関節
     */
    public static NaoJointIdentifier LLJ6 = new NaoJointIdentifier("llj6", Angle.createAngleAsDegree(-45), Angle.createAngleAsDegree(25), Align.LEFT, true);

    /**
     *
     * @see jp.seraph.jsade.model.JointIdentifier#toString()
     */
    public String toString() {
        return mName;
    }

    /**
     *
     * @see jp.seraph.jsade.model.JointIdentifier#toJointName()
     */
    public String toJointName() {
        return mName;
    }

    /**
     * @see jp.seraph.jsade.model.JointIdentifier#getMaxAngle()
     */
    public Angle getMaxAngle() {
        return mMaxAngle;
    }

    /**
     * @see jp.seraph.jsade.model.JointIdentifier#getMinAngle()
     */
    public Angle getMinAngle() {
        return mMinAngle;
    }

    public Align getAlign() {
        return mAlign;
    }

    public NaoJointIdentifier getMirrorIdentifier() {
        return IDENTIFIER_MAP.get(mMirrorIdentifier);
    }

    public boolean shouldMinusAsMirror() {
        return mShouldMinus;
    }
}
